from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument,ExecuteProcess
from launch.actions import IncludeLaunchDescription
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution,PythonExpression

from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare   
import os
import yaml

if os.path.exists('config.yaml'):
    config_file = 'config.yaml'
    with open(config_file, 'r', encoding='utf-8') as file:
        config = yaml.safe_load(file)
    
    # 如果 config.yaml 中没有 px4_paths，则读取 config_template.yaml
    if "px4_paths" not in config or not config["px4_paths"]:
        if os.path.exists('config_template.yaml'):
            with open('config_template.yaml', 'r', encoding='utf-8') as file:
                config = yaml.safe_load(file)
        else:
            raise KeyError("config.yaml 中没有 px4_paths 且找不到 config_template.yaml")

# 如果 config.yaml 不存在，则读取 config_template.yaml
elif os.path.exists('config_template.yaml'):
    config_file = 'config_template.yaml'
    with open(config_file, 'r', encoding='utf-8') as file:
        config = yaml.safe_load(file)
else:
    raise FileNotFoundError("找不到配置文件：config.yaml 或 config_template.yaml")

config["px4_paths"]

def generate_launch_description():
    px4_path = LaunchConfiguration('px4_path')
    uav_pose = LaunchConfiguration('uav_pose')
    uav_id = LaunchConfiguration('uav_id')
    uav_num = LaunchConfiguration('uav_num')
    sim_model = LaunchConfiguration('sim_model')
    sim_world = LaunchConfiguration('sim_world')


    px4_path_launch_arg = DeclareLaunchArgument(
        'px4_path',
        default_value= config["px4_paths"],
        description='PX4 Firmware Path'
    )

    uav_pose_launch_arg = DeclareLaunchArgument(
        'uav_pose',
        default_value= '0,0,0',
        description='UAV Pose'
    )

    uav_id_launch_arg = DeclareLaunchArgument(
        'uav_id',
        default_value= 'uav1',
        description='UAV ID'
    )

    uav_num_launch_arg = DeclareLaunchArgument(
        'uav_num',
        default_value= '0',
        description='UAV Number'
    )
    sim_model_launch_arg = DeclareLaunchArgument(
        'sim_model',
        default_value= 'gz_x500',
        description='Simulation Model'
    )

    sim_world_launch_arg = DeclareLaunchArgument(
        'sim_world',
        default_value= 'default',
        description='Simulation World'
    )

    #执行 PX4_GZ_WORLD=default PX4_GZ_STANDALONE=1 PX4_SYS_AUTOSTART=4001 PX4_SIM_MODEL=gz_x500 PX4_GZ_MODEL_POSE="2,3" PX4_UXRCE_DDS_NS=uav2 ./build/px4_sitl_default/bin/px4 -i 1
    
    px4_start = ExecuteProcess(
            cmd=[
                PathJoinSubstitution([px4_path, 'build', 'px4_sitl_default', 'bin', 'px4']),
                '-i',
                uav_num
            ],
            additional_env={
                'PX4_GZ_STANDALONE': '1',
                'PX4_SYS_AUTOSTART': '4001',
                'PX4_SIM_MODEL': sim_model,
                'PX4_GZ_WORLD': sim_world,
                'PX4_GZ_MODEL_POSE': uav_pose,
                'PX4_GZ_NS': uav_id,
                'PX4_UXRCE_DDS_NS': uav_id,
                'PX4_USE_SIM_TIME': '1'  # 设置使用仿真时间
            },
            cwd=px4_path,  # 根据实际情况设置工作目录
            output='screen'
        )
 

    return LaunchDescription([
        px4_path_launch_arg,
        uav_pose_launch_arg,
        uav_id_launch_arg,
        uav_num_launch_arg,
        sim_model_launch_arg,
        sim_world_launch_arg,
        px4_start
    ])
